#region File information

/**
 * File: CollisionObject.cs
 * Date: 29-03-2008
 * Auth: Gerard v/d Lei
 * 
 * Desc: File which contains the CollisionObject which is the global world
 *       representation of a collision hull
 */

#endregion

using Microsoft.Xna.Framework;

namespace Gremlin.Collision
{
    /// <summary>
    /// This is the interface between the collision hull and the game object.
    /// It contains a reference to the CollisionHull and gets position, rotation, scale
    /// etc each frame from a game object.
    /// All collision calls should be made with this class. It does the transformations
    /// of the objects and calculates the collisions.
    /// </summary>
    public class CollisionObject
    {
        #region Attributes
        private CollisionHull _localCollisionHull;
        private CollisionHull _globalCollisionHull;
        //Input
        private float _timeStep, _weight, _frictionFactor;
        private Vector3 _iPosition, _iDelta, _iScale;
        private Quaternion _iRotation, _iDeltaRotation, _iResultRotation;
        private Matrix _iTransform;
        //Output
        private Vector3 _oDelta;
        private Quaternion _oDeltaRotation;
        //Damage registering
        private float _oImpactForce, _oRayDamage;
        #endregion

        /// <summary>
        /// The constructor, which makes a new CollisionObject based on a CollisionHull
        /// </summary>
        /// <param name="hull">The hull used for this CollisionObject</param>
        public CollisionObject(ref CollisionHull hull)
        {
            this._localCollisionHull = hull;
            _globalCollisionHull = null;
        }

        /// <summary>
        /// Call which signals that the global instance of this collision object is invalid
        /// </summary>
        private void InvalidateGlobalInstance()
        {
            _globalCollisionHull = null;
        }

        /// <summary>
        /// Set states that do not change often during game
        /// </summary>
        /// <param name="scale">Global scale of the object compared to local scale</param>
        /// <param name="weight">Weight of object</param>
        public void SetSteadyStates(Vector3 scale, float weight, float frictionFactor)
        {
            _frictionFactor = frictionFactor;
            _weight = weight;
            _iScale = scale;
        }

        /// <summary>
        /// Temporary interface with this class.
        /// Sets position, rotation, and speed/direction of the object
        /// </summary>
        /// <param name="position">Position of object</param>
        /// <param name="rotation">Orientation of object</param>
        /// <param name="delta">Speed and direction of movement of object</param>
        /// <param name="deltaRotation">Direction and speed of rotation</param>
        /// <param name="timeStep">Amount of elapsed time</param>
        public void Set(Vector3 position, Quaternion rotation, Vector3 delta, Quaternion deltaRotation, float timeStep)
        {
            InvalidateGlobalInstance();
            _oImpactForce = 0;
            _timeStep = timeStep;
            _iPosition = position;
            _iRotation = rotation;
            _oDelta = _iDelta = delta;
            _oDeltaRotation = _iDeltaRotation = deltaRotation;
            _iResultRotation = Quaternion.Concatenate(_iRotation, (_iDeltaRotation * _timeStep));
            _iTransform = Matrix.CreateScale(_iScale) *
                          Matrix.CreateFromQuaternion(_iResultRotation) *
                          Matrix.CreateTranslation(_iPosition + (_iDelta * _timeStep));
        }

        /// <summary>
        /// Part of temportary interface.
        /// Return change in speeds
        /// </summary>
        /// <param name="position"></param>
        /// <param name="rotation"></param>
        /// <param name="delta"></param>
        /// <param name="deltaRotation"></param>
        public void Get(out Vector3 delta, out Quaternion deltaRotation, out float impactForce, out float rayDamage)
        {
            delta = _oDelta;
            deltaRotation = _oDeltaRotation;
            impactForce = _oImpactForce;
            rayDamage = _oRayDamage;
        }

        /// <summary>
        /// Computes the intersection of a ray with the collision object. Collision object doesn't
        /// have to be global.
        /// </summary>
        /// <param name="origin">Origin of the ray</param>
        /// <param name="delta">Direction and speed of the ray</param>
        /// <param name="point">Point of intersection</param>
        /// <returns>True when there is an intersection, false otherwise</returns>
        public bool RayIntersection(Vector3 origin, Vector3 delta, out Vector3 point)
        {
            origin -= _iPosition;
            delta -= _iDelta * _timeStep;
            Quaternion iRotation = Quaternion.Inverse(_iResultRotation);
            origin = Vector3.Transform(origin, iRotation);
            delta = Vector3.Transform(delta, iRotation);
            origin /= _iScale;
            delta /= _iScale;
            bool res = _localCollisionHull.RayIntersection(origin, delta, out point);
            point *= _iScale;
            point = Vector3.Transform(point, _iResultRotation);
            point += _iPosition;
            return res;
        }

        /// <summary>
        /// Ray damage is tho total amount of damage due to ray intersection that is inflicted
        /// for example by bullets
        /// </summary>
        public float RayDamage
        {
            get { return _oRayDamage; }
            set { _oRayDamage = value; }
        }

        /// <summary>
        /// Fast range checking, very primitive, but fast
        /// </summary>
        /// <param name="co0">One of the two object to check distance between</param>
        /// <param name="co1">One of the two object to check distance between</param>
        /// <returns>True when close enough that collision is possible, false otherwise</returns>
        public static bool FastRangeCheck(CollisionObject co0, CollisionObject co1)
        {
            float rangeSquared = (co0._iPosition - co1._iPosition).LengthSquared();
            float maxScale0 = MathHelper.Max(MathHelper.Max(co0._iScale.X, co0._iScale.Y), co0._iScale.Z);
            float maxScale1 = MathHelper.Max(MathHelper.Max(co1._iScale.X, co1._iScale.Y), co1._iScale.Z);
            float allowedRange = maxScale0 * co0._localCollisionHull.Radius + maxScale1 * co1._localCollisionHull.Radius;
            float movement = (co0._iDelta).Length() * co0._timeStep + (co1._iDelta).Length() * co1._timeStep;
            if (rangeSquared < System.Math.Pow((allowedRange + movement), 2))
                return true;
            else
                return false;
        }

        /// <summary>
        /// Does the collision detection and responce between two objects
        /// </summary>
        /// <param name="co0">First collision object</param>
        /// <param name="co1">Second collision object</param>
        /// <returns>True when collision occoured, false otherwise</returns>
        public static bool CollisionObjectIntersection(CollisionObject co0, CollisionObject co1)
        {
            Vector3 centerPoint;
            //First contruct global instances if this is not yet done
            if (co0._globalCollisionHull == null)
                co0._globalCollisionHull = new CollisionHull(co0._localCollisionHull, co0._iTransform, co0._iResultRotation);
            if (co1._globalCollisionHull == null)
                co1._globalCollisionHull = new CollisionHull(co1._localCollisionHull, co1._iTransform, co1._iResultRotation);

            //Check for intersection AKA collision
            if (CollisionHull.CollisionHullIntersection(co0._globalCollisionHull, co1._globalCollisionHull, out centerPoint))
            {
                //Collision will happen in coming frame
                CollisionObject.CollisionObjectResponce(co0, co1, centerPoint);

                //Compute new speeds for next collision or output
                co0._oDelta = co0._iDelta;
                co1._oDelta = co1._iDelta;
                co0._oDeltaRotation = co0._iDeltaRotation;
                co1._oDeltaRotation = co1._iDeltaRotation;
                return true;
            }
            return false;
        }

        /// <summary>
        /// Performs collision responce between two object that intersected
        /// </summary>
        /// <param name="co0">First collision object</param>
        /// <param name="co1">Second collision object</param>
        /// <param name="intersectPoint">Main point of intersection</param>
        private static void CollisionObjectResponce(CollisionObject co0, CollisionObject co1, Vector3 intersectPoint)
        {
            Vector3 impactPoint0, impactPoint1;
            TPlane impactPlane0, impactPlane1;
            //Get velocity at collion point
            Vector3 totalVelocity0 = (co0._iDelta + co0.GetAngularVelocity(intersectPoint));
            Vector3 totalVelocity1 = (co1._iDelta + co1.GetAngularVelocity(intersectPoint));
            //Get total velocity between both points of the two objects
            Vector3 deltaVelocity = totalVelocity1 - totalVelocity0;

            //Find the angle of the surface where collision happens
            if (!co0._globalCollisionHull.RayIntersection(intersectPoint - deltaVelocity, deltaVelocity, out impactPoint0, out impactPlane0))
            {
                impactPoint0 = intersectPoint;
                impactPlane0._normal = Vector3.Normalize(-deltaVelocity);
            }
            if (!co1._globalCollisionHull.RayIntersection(intersectPoint + deltaVelocity, -deltaVelocity, out impactPoint1, out impactPlane1))
            {
                impactPoint1 = intersectPoint;
                impactPlane1._normal = Vector3.Normalize(deltaVelocity);
            }

            //Lets convert from velocities to forces
            Vector3 deltaForce = totalVelocity1 * co1._weight  - totalVelocity0 * co0._weight;
            //Reduce forces due to the angles of the surfaces of collision
            float deflectFactor = -Vector3.Dot(impactPlane0._normal, impactPlane1._normal);
            deltaForce *= deflectFactor;
            float deltaForceLength = deltaForce.Length();
            //Compute the forces by using surface normal, with length of the delta force
            Vector3 deltaForce0 = -impactPlane0._normal * deltaForceLength * co0._frictionFactor;
            Vector3 deltaForce1 = -impactPlane1._normal * deltaForceLength * co1._frictionFactor;
            //Delta force is the total impact force, safe for damage, friction purposes
            co0._oImpactForce += deltaForce0.Length();
            co1._oImpactForce += deltaForce1.Length();

            //Calculate the amount transfert to motion, by comparing angle of force to angle to center of gravity
            Vector3 deltaCenterGravity0 = co0._iPosition - impactPoint0;
            deltaCenterGravity0.Normalize();
            float motionPart0 = 1 - Vector3.Cross(-impactPlane0._normal, deltaCenterGravity0).Length();
            Vector3 deltaCenterGravity1 = co1._iPosition - impactPoint1;
            deltaCenterGravity1.Normalize();
            float motionPart1 = 1 - Vector3.Cross(-impactPlane1._normal, deltaCenterGravity1).Length();

            //Forces that change motion
            Vector3 deltaMotionForce0 = deltaForce0 * motionPart0;
            Vector3 deltaMotionForce1 = deltaForce1 * motionPart1;

            //Convert from forces to velocity
            Vector3 deltaMotionVelocity0 = deltaMotionForce0 / co0._weight;
            Vector3 deltaMotionVelocity1 = deltaMotionForce1 / co1._weight;

            //Get new speeds
            co0._iDelta += deltaMotionVelocity0;
            co1._iDelta += deltaMotionVelocity1;

            //Do the rotation, first get resulting forces
            Vector3 deltaAngularForce0 = deltaForce0 * (1 - motionPart0);
            Vector3 deltaAngularForce1 = deltaForce1 * (1 - motionPart1);
            //Calc Rotation axis
            Vector3 rotationAxis0 = Vector3.Cross(-impactPlane0._normal, deltaCenterGravity0);
            rotationAxis0.Normalize();
            Vector3 rotationAxis1 = Vector3.Cross(-impactPlane1._normal, deltaCenterGravity1);
            rotationAxis1.Normalize();
            //Get distance from rotation axis
            float distance0 = Vector3.Cross(rotationAxis0, impactPoint0 - co0._iPosition).Length();
            float distance1 = Vector3.Cross(rotationAxis1, impactPoint1 - co1._iPosition).Length();
            //Distance times force is applied force, divided by inertia is rotation speed (intertia constant is estimated)
            float rotationSpeed0 = distance0 * deltaAngularForce0.Length() / (co0._weight * co0._globalCollisionHull.Radius * co0._globalCollisionHull.Radius * 0.4f);
            float rotationSpeed1 = distance1 * deltaAngularForce1.Length() / (co1._weight * co1._globalCollisionHull.Radius * co1._globalCollisionHull.Radius * 0.4f);

            //Resulting angular velocity
            co0._iDeltaRotation *= Quaternion.CreateFromAxisAngle(rotationAxis0, rotationSpeed0);
            co1._iDeltaRotation *= Quaternion.CreateFromAxisAngle(rotationAxis1, rotationSpeed1);
        }

        /// <summary>
        /// Get speed of a point rotation around center of gravity (0,0,0)
        /// </summary>
        /// <param name="point">Point to get speed from</param>
        /// <returns></returns>
        private Vector3 GetAngularVelocity(Vector3 point)
        {
            //This should be distance from vector times length of vector times (2*)pi
            Vector3 lPoint = point - _iPosition;
            return MathHelper.Pi * Vector3.Cross(new Vector3(_iDeltaRotation.X, _iDeltaRotation.Y, _iDeltaRotation.Z), lPoint);
        }

        /// <summary>
        /// Applies an amount of impuls force at point (which should lie on the collision object)
        /// towards an certain direction
        /// </summary>
        /// <param name="point">Point to which to apply the force</param>
        /// <param name="direction">Normalized direction of the force</param>
        /// <param name="force">Amount of force to applie</param>
        public void ImpulsForce(Vector3 point, Vector3 direction, float force)
        {
            if (_globalCollisionHull == null)
                _globalCollisionHull = new CollisionHull(_localCollisionHull, _iTransform, _iResultRotation);

            //Find surface to apply force to
            //Find the angle of the surface where collision happens
            Vector3 impactPoint;
            TPlane impactPlane;
            if (!_globalCollisionHull.RayIntersection(point - direction, direction*2, out impactPoint, out impactPlane))
            {
                impactPoint = point;
                impactPlane._normal = Vector3.Normalize(-direction);
            }

            //Reduce forces due to the angles of the surfaces of collision
            float deflectFactor = -Vector3.Dot(impactPlane._normal, direction);
            float remainingForce = force * deflectFactor;
            //Calculate applied force
            Vector3 deltaForce = -impactPlane._normal * remainingForce;

            //Calculate the amount transfert to motion, by comparing angle of force to angle to center of gravity
            Vector3 deltaCenterGravity = _iPosition - impactPoint;
            deltaCenterGravity.Normalize();
            float motionPart = 1 - Vector3.Cross(-impactPlane._normal, deltaCenterGravity).Length();
            //Forces that change motion
            Vector3 deltaMotionForce = deltaForce * motionPart;
            //Convert from forces to velocity
            Vector3 deltaMotionVelocity = deltaMotionForce / _weight;
            //Set new velocity
            _iDelta += deltaMotionVelocity;
            //Do the rotation, first get resulting forces
            Vector3 deltaAngularForce = deltaForce * (1 - motionPart);
            //Calc Rotation axis
            Vector3 rotationAxis = Vector3.Cross(-impactPlane._normal, deltaCenterGravity);
            rotationAxis.Normalize();
            //Get distance from rotation axis
            float distance = Vector3.Cross(rotationAxis, impactPoint - _iPosition).Length();
            //Distance times force is applied force, divided by inertia is rotation speed (intertia constant is estimated)
            float rotationSpeed = distance * deltaAngularForce.Length() / (_weight * _globalCollisionHull.Radius * _globalCollisionHull.Radius * 0.4f);
            //Resulting angular velocity
            _iDeltaRotation *= Quaternion.CreateFromAxisAngle(rotationAxis, rotationSpeed);

            //Compute new speeds for next collision or output
            _oDelta = _iDelta;
            _oDeltaRotation = _iDeltaRotation;
        }
    }
}
